package simulator;

import java.awt.Color;
import java.awt.Graphics;
import java.awt.Graphics2D;
import java.awt.Point;
import java.awt.RenderingHints;
import java.awt.image.BufferedImage;
import java.io.File;
import java.io.IOException;
import java.io.PrintWriter;
import java.util.ArrayList;

import javax.imageio.ImageIO;
import javax.swing.JComponent;
import javax.swing.JFrame;

import pteam.AndrewActor;

/**
 * This simulates the simple control api exposed by the Roomba and Eric's code.
 * Simulator initiates all interactions in the world, and acts as the Robot's
 * actuators. Also, the Simulator allows small Actors to be attached to control
 * robot motion/action
 */
public class Simulator extends JComponent {
	public static enum Condition {
		DISTANCE, ANGLE
	};

	public static final double TIMESTEP = 0.01; // update 100 times a second

	protected ArrayList<Point> locations = new ArrayList<Point>();
	protected ArrayList<Actor> actors = new ArrayList<Actor>();

	protected boolean acting = false;
	protected Condition condition;
	protected double[] condition_args;

	protected int speed = 100;
	protected double omega = Math.PI / 4.;
	protected double angle;
	protected double x = 50, y = 50;
	protected int direction = +1; // +1 = forwards, -1 = backwards, 0 = turning
									// l/r
	protected int rot_direction = 0; // +1 = clockwise, -1 = ccw

	public static double time = 0.0;

	public Environment env = new Environment();
	public Robot robot = new Robot(this);
	public static PrintWriter out = new PrintWriter(System.out);

	public void paintComponent(Graphics g) {
		// if (locations.size() <= 1) {
		// g.setColor(Color.RED);
		// g.fillRect(100, 100, 100, 100);
		// g.setColor(Color.BLACK);
		// g.drawString("Epic... Fail.", 150, 150);
		// return;
		// }
		Graphics2D g2 = (Graphics2D) g;
		g2.setRenderingHint(RenderingHints.KEY_ANTIALIASING,
				RenderingHints.VALUE_ANTIALIAS_ON);

		// bg
		g.setColor(Color.WHITE);
		g.fillRect(0, 0, getWidth(), getHeight());
		g.setColor(Color.BLACK);

		// paint the enviornment
		for (int i = 0; i < env.getObjects().size(); i++) {
			env.get(i).paint(g);
		}

		g.setColor(Color.BLACK);

		// paint lines between each location
		/*
		 * g.fillOval((int)locations.get(locations.size()-1).getX()-2,
		 * (int)locations.get(locations.size()-1).getY()-2, (int)4, (int)4);
		 */
		for (int i = 0; i < locations.size() - 1; i++) {
			if (locations.size() > 1)
				g.drawLine((int) locations.get(i).getX(), (int) locations
						.get(i).getY(), (int) locations.get(i + 1).getX(),
						(int) locations.get(i + 1).getY());
		}

		// draw robot sensors
		for (int i = 0; i < robot.getSensors().size(); i++) {
			robot.getSensors().get(i).paint(g);
		}
		g.setColor(Color.BLACK);

		// draw robot
		g.drawOval((int) /* locations.get(locations.size()-1).getX() */x - 5,
				(int) /* locations.get(locations.size()-1).getY() */y - 5, 10, 10);

		g.setColor(Color.RED);
		// robot orientation drawn as a red line
		g.drawLine((int) locations.get(locations.size() - 1).getX(),
				(int) locations.get(locations.size() - 1).getY(),
				(int) (locations.get(locations.size() - 1).getX() + 9 * Math
						.cos(angle)),
				(int) (locations.get(locations.size() - 1).getY() + 9 * Math
						.sin(angle)));
		g.setColor(Color.BLACK);

		repaint();
	}

	public void trackPosition() {
		new Thread() {
			public void run() {
				while (simulationContinues()) {
					if (acting) {// we only change position when we are waiting
									// for some event
						x += speed * Math.cos(angle) * direction * TIMESTEP;
						y += speed * Math.sin(angle) * direction * TIMESTEP;
						angle += omega * rot_direction * TIMESTEP;
					}

					// physically stop any moving through walls
					// TODO: allow dragging across walls
					for (int i = 0; i < env.getObjects().size(); i++) {
						while (env.get(i).intersects(x, y)) {
							x -= speed * Math.cos(angle) * direction * TIMESTEP;
							y -= speed * Math.sin(angle) * direction * TIMESTEP;
						}
					}

					env.updateLocation(x, y, angle);

					if ((int) (time * (1 / TIMESTEP)) % (int) (10 / TIMESTEP) == 0) {
						Simulator.out.println("time: " + (int) time);
					}
					if ((int) (time * (1 / TIMESTEP)) % (int) (.1 / TIMESTEP) == 0) {
						locations.add(new Point((int) x, (int) y));
					}

					time += TIMESTEP;

					// hand off to another thread
					try {
						Thread.sleep(1);
					} catch (InterruptedException e) {
						// TODO Auto-generated catch block
						e.printStackTrace();
					}

				}
			}
		}.start();
	}

	public void readSensors() {
		new Thread() {
			public void run() {
				while (simulationContinues()) {
					// prepare sensor data
					Sensor[] sensors = robot.updateSensors();
					for (Actor a : actors) {
						a.updateSensors(sensors);
					}
					// hand off to another thread
					try {
						Thread.sleep(1);
					} catch (InterruptedException e) {
						// TODO Auto-generated catch block
						e.printStackTrace();
					}

				}
			}
		}.start();
	}

	public void actLoop() {
		new Thread() {
			public void run() {
				while (simulationContinues()) {
					// controllers
					try {
						Thread.sleep(1);
					} catch (InterruptedException e) {
						// TODO Auto-generated catch block
						e.printStackTrace();
					}
					for (Actor a : actors) {
						if (!acting) {
							a.act(robot);
						}
					}
				}
			}
		}.start();
	}

	public void waitTimingLoop() {
		new Thread() {
			public void run() {
				while (simulationContinues()) {
					// controllers
					try {
						Thread.sleep(10);
					} catch (InterruptedException e) {
						// TODO Auto-generated catch block
						e.printStackTrace();
					}
					// wait distance
					if (acting && condition == Condition.DISTANCE) {
						if (Math.sqrt((x - condition_args[0])
								* (x - condition_args[0])
								+ (y - condition_args[1])
								* (y - condition_args[1])) >= condition_args[2]) {
							acting = false; // distance conditions were met.
							condition_args = new double[0];
							repaint();
						}// waitangle
					}
					// waitangle
					else if (acting && condition == Condition.ANGLE) {
						if (condition_args[1] == 1.0 ? angle >= condition_args[0]
								: angle <= condition_args[0]) {
							acting = false; // angle conditions were met
							condition_args = new double[0];
							repaint();
						}
					}

					if (!(Math.abs(direction) + Math.abs(rot_direction) != 2)) {
						throw new RuntimeException(
								"Concurrent turning and driving is an illegal action");
					}
				}
			}
		}.start();
	}

	/**
	 * Hands control over to the simulator until the robot completes the course
	 * or crashes in a non-recoverable fashion. (see simulationContinues);
	 * 
	 * @throws RuntimeException
	 */
	public void mainLoop() {
		trackPosition();
		readSensors();
		actLoop();
		// waitTimingLoop();
		// while (simulationContinues()) {
		//
		// // actuate
		// if (!acting)
		// robot.act(this);
		//
		// try {
		// Thread.sleep(1);
		// } catch (InterruptedException e) {
		// // TODO Auto-generated catch block
		// e.printStackTrace();
		// }
		//
		// }
	}

	/**
	 * Simulation ends when
	 * 
	 * @return
	 */
	public boolean simulationContinues() {
		for (PhysicalObject o : env.getObjects()) {
			if (o.getClass().toString().equals(Beacon.class.toString())
					&& Math.abs(o.getX() - x) < 5 && Math.abs(o.getY() - y) < 5) {
				Simulator.out.println("GAME OVER");
				return false;
			}
		}
		return true;
	}

	// add an actor to the Robot
	public void addActor(Actor a) {
		actors.add(a);
	}

	public void removeActor(Actor a) {
		actors.remove(a);
	}

	public void setSpeed(int speed) {
		this.speed = speed;
	}

	public void goForward() {
		direction = +1;
		rot_direction = 0;
	}

	public void goBackward() {
		direction = -1;
		rot_direction = 0;
	}

	public void turnLeft() {
		direction = 0;
		rot_direction = -1;
	}

	public void turnRight() {
		direction = 0;
		rot_direction = +1;
	}

	public void stop() {
		direction = 0;
		rot_direction = 0;
	}

	/**
	 * @param degrees
	 * @condition degrees>=0 After turning left or right is initated, Continue
	 *            turn until the param condtion in degrees is met.
	 * 
	 */
	public void waitAngle(int degrees) {
		if (direction != 0 || rot_direction == 0) {
			throw new RuntimeException(
					"waitAngle must be preceded by a turnLeft or "
							+ "turnRight function call");
		}

		double ang = ((double) degrees) * 2 * Math.PI / 360.;
		condition = Condition.ANGLE;
		condition_args = new double[] { angle + ang * rot_direction,
				rot_direction == 1 ? 1.0 : 0.0 };
		// prevents any commands from being sent until the condition is met
		acting = true;

		while (acting && condition == Condition.ANGLE) {
			if (condition_args[1] == 1.0 ? angle >= condition_args[0]
					: angle <= condition_args[0]) {
				acting = false; // angle conditions were met
				condition_args = new double[0];
				// repaint();
			}
			try {
				Thread.sleep(1);
			} catch (InterruptedException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
		}

	}

	/**
	 * @param dist
	 * @condition dist>=0 After forward/reverse motion initiated, continues
	 *            motion until param distance is achieved.
	 */
	public void waitDistance(int dist) {
		if (direction == 0 || rot_direction != 0) {
			throw new RuntimeException(
					"waitDistance must be preceded by a goForward"
							+ " or goBackward function call");
		}

		condition = Condition.DISTANCE;
		condition_args = new double[] { x, y, dist };
		// prevents any commands from being sent until the condition is met
		acting = true;

		while (acting && condition == Condition.DISTANCE) {
			if (Math.sqrt((x - condition_args[0]) * (x - condition_args[0])
					+ (y - condition_args[1]) * (y - condition_args[1])) >= condition_args[2]) {
				acting = false; // distance conditions were met.
				condition_args = new double[0];
				// repaint();
			}// waitangle

			try {
				Thread.sleep(1);
			} catch (InterruptedException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
		}
	}

	public void sampleConfiguration() {
		Simulator sim = this;
		sim.env = EnvironmentFactory.getDefaultConfiguration();
		sim.robot = RobotFactory.getDefaultConfiguration(sim.env, this);
	}

	public void customConfiguration(String[] args) {
		Simulator sim = this;
		if (args[0] != null)
			try {
				sim.env = EnvironmentFactory.loadConfiguration(args[0]);
			} catch (IOException e) {
				sim.env = EnvironmentFactory.getDefaultConfiguration();
				e.printStackTrace();
			}
		else
			sim.env = EnvironmentFactory.getDefaultConfiguration();
		sim.robot = RobotFactory.getDefaultConfiguration(sim.env, this);
		retrieveStart();
	}

	public void setEnvironment(Environment env) {
		this.env = env;
		retrieveStart();
	}

	private void retrieveStart() {
		x = env.getRobotX();
		y = env.getRobotY();
	}

	/**
	 * Save the simulator as an image to the screen.
	 */
	public void save(String path) {
		try {
			BufferedImage img = new BufferedImage(1000, 300,
					BufferedImage.TYPE_INT_ARGB);
			this.paintComponent(img.createGraphics());
			File outputFile = new File(path);
			ImageIO.write(img, "PNG", outputFile);
		} catch (Exception e) {
			e.printStackTrace(Simulator.out);
		}
	}

	public static void main(String[] args) {
		JFrame home = new JFrame("Simulator");

		Simulator sim = new Simulator();

		// ADD ACTOR(S)
		Actor b = new AndrewActor();
		sim.customConfiguration(new String[] { "maze/" + b.getMaze() + ".maze" });
		sim.addActor(b);

		// This actor will serve telemetry data for Dashboard and client apps
		Actor c = new SnitchActor();
		sim.addActor(c);
		((SnitchActor) c).start();

		home.getContentPane().add(sim);
		home.setLocation(200, 200);
		home.setSize(1000, 300);
		home.setVisible(true);
		home.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);

		// Start Simulation
		sim.mainLoop();

	}
}